package physics;

/**
 * 
 * @author Dylan
 *
 * Uses the RK4 integrator to find the new position of an object given initial position,
 * acceleration at points in time, initial velocity, and time period. Used as a more
 * accurate alternative to other integrators.
 *
 */

public class RK4 {
	public RK4(){}
	
	public Position integrate(Position position, double t, double dt, double acceleration){	//Heart of the RK4 integrator - I don't know what most of this is
		Derivative a = evaluate(position, t, 0, new Derivative(0, 0), acceleration);
		Derivative b = evaluate(position, t+dt*0.5, dt*0.5, a, acceleration);
		Derivative c = evaluate(position, t+dt*0.5, dt*0.5, b, acceleration);
		Derivative d = evaluate(position, t+dt, dt, c, acceleration);
		
		double dpdt = 1.0 / 6.0 * (a.dp + 2.0*(b.dp + c.dp) + d.dp);
		double dvdt = 1.0 / 6.0 * (a.dv + 2.0*(b.dv + c.dv) + d.dv);
		
		position.pos += dpdt * dt;
		position.v += dvdt * dt;
		
		return position;
	}
	
	public double acceleration(Position position, double t){	//Calculate all acceleration here - modify as needed
		double f = position.a;
		System.out.println(position.a);
		return f;
	}
	
	public Derivative evaluate(Position initial, double t, double dt, Derivative d, double acceleration){	//Calculate new position based on change over time
		Position position = new Position(initial.pos + d.dp * dt, initial.v + d.dv * dt);	//New state influenced by derivatives of pos and v
		return new Derivative(position.v, acceleration);//acceleration(position, t));	//Calculate new derivative for new position
	}
}
